#include "precompiled.h"
#include "Util.h"
#include "Config.h"
#include "Track.h"

using namespace boost::numeric::ublas;

Track::Track() :
	Freq (75e3),
	Current (0.1 * M_SQRT2),
	Filename ("default.track")
{
	json::grammar<char>::object &o = Config::Get("Track");
	Config::TryAssign(Freq, "Freq", o);
	Config::TryAssign(Current, "Current", o);
	Config::TryAssign(Filename, "Filename", o);

	Load();
}

void Track::Load() {
	std::ifstream in(Filename.c_str());
	if(!in.is_open()) {
		std::ostringstream msg;
		msg << "Failed to open file: " << Filename;
		throw std::runtime_error(msg.str());
	}
	while(in.good()) {
		char buf[BUFSIZ];
		in.getline(buf, sizeof(buf));
		double x, y;
		if(sscanf_s(buf, "[%lf,%lf]\n", &x, &y) != 2) {
			break;
		}
		ublas::vector<double> p(2);
		p[0] = x;
		p[1] = y;
		mPath.push_back(p);
	}
}

void Track::Draw() {
	glBegin(GL_LINE_STRIP);
	for(point_list::const_iterator it = mPath.begin(); it != mPath.end(); it++) {
		const point_list::value_type &p = *it;		// convenience reference
		glVertex2d(p[0], p[1]);
	}
	glEnd();
}

void Track::Print() {
	for(point_list::const_iterator it = mPath.begin(); it != mPath.end(); it++) {
		const point_list::value_type &p = *it;	// convenience reference
		std::cerr << p << "\n";
	}
}

// return a 2 element vector
ublas::vector<double> Track::GetStartPosition() const {
	if(mPath.size() == 0) {
		throw std::runtime_error("Cannot compute start position: track does not contain any points");
	}
	return mPath[0];
}

// return a 2 element vector
ublas::vector<double> Track::GetStartHeading() const {
	if(mPath.size() < 2) {
		throw std::runtime_error("Cannot compute start heading: track contains less than two points");
	}
	ublas::vector<double> dir = mPath[1] - mPath[0];
	dir /= norm_2(dir);
	return dir;
}


// calculates the magnitude of the magnetic field at the requested point assuming
// a constant current of 1A is flowing through the wire. p0 should be 3 dimensional
// http://www.ac.wwu.edu/~vawter/PhysicsNet/Topics/MagneticField/MFStraitWire.html
// Note that returned value is in Tesla
ublas::vector<double> Track::GetFieldAt(const ublas::vector<double> &p) const {
	typedef ublas::vector<double> vec;
	assert(p.size() == 3);
	
	vec field(3);
	field[0] = 0;
	field[1] = 0;
	field[2] = 0;

	// for each segment in the path
	for(int i = 1; i < (int)mPath.size(); i++) {
		vec p1(3);
		vec p2(3);

		project(p1, range(0, 2)) = mPath[i - 1];
		p1[2] = 0;
		project(p2, range(0, 2)) = mPath[i];
		p2[2] = 0;

		//std::cerr << p1 << " " << p2 << "\n";
		if(norm_2(p2 - p1) < 1.0) {
			continue;
		}

		// compute distance from track segment to requested point
		// http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
		double d = norm_2(Util::cross_product(p - p1, p - p2)) / norm_2(p2 - p1);
		//std::cerr << p << ": " << d << "\n";
		if(d == 0) {
			continue;
		}

		// calculate phi1 and phi2 using angle between two vectors
		// http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
		// angle = acos(v1 dot v2) where v1 and v2 are normalized
		double cos_phi1 = inner_prod((p - p1) / norm_2(p - p1), (p2 - p1) / norm_2(p2 - p1));
		double cos_phi2 = inner_prod((p - p2) / norm_2(p - p2), (p1 - p2) / norm_2(p1 - p2));

		//std::cerr << p << ": " << 180 / M_PI * acos(cos_phi1) << ", " << 180 / M_PI * acos(cos_phi2) << "\n";

		// convert from cm to meters (divide d by 100)
		double B = (cos_phi1 + cos_phi2) / d;

		// B is the (unscaled) magnitude. Now we need to create the direction vector.
		// The direction vector is orthogonal to the plane formed by the line and the point
		// Therefore it will be the cross product of two unit vectors in the plane
		vec rhat = Util::cross_product(p2 - p1, p - p1);
		//std::cerr << rhat << "\n";
		field += B * rhat / norm_2(rhat);
	}

	return field * 1e-7 * Current;
}